#include"guiapi.h"
#define FILE_PATH "/home/tcr/sh_file/"
RosGuiApi::RosGuiApi():rate_(10.0),move_base_client_("/move_base", true)
{}
RosGuiApi::~RosGuiApi()
{}


bool RosGuiApi::init()
{
    ros::NodeHandle nh;
    initial_position_pub_= nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 100, true);
    move_base_status_sub_ = nh_.subscribe("/move_base/status", 10, &RosGuiApi::moveBaseResultCallback, this);
    encoder_pub_=nh_.advertise<tcr100::VehicleEncoder>("/vehicleEncoder", 100, true);
    cmd_vel_ = nh_.subscribe("/cmd_vel", 10, &RosGuiApi::twistCmdVelCallback, this);
    move_base_path_sub_=nh_.subscribe("/move_base/NavfnROS/plan", 10, &RosGuiApi::gloablPathLenghtCallback, this);
    move_base_goal_sub_=nh_.subscribe("/move_base/goal", 10, &RosGuiApi::movebaseGoalCallback, this);
    pose_sub_=nh_.subscribe("/amcl_pose", 10, &RosGuiApi::robotPoseCallback, this);
    goal_pub_=nh_.advertise<tcr100::VehicleGoal>("/vehicle_goal", 100, true);
    return true;
}

//建图选择
int RosGuiApi::setMapSolver(struct UserInfo userInfo,enum E_MAP_SOLVER solverIn,int laserCount,int &returnFlag)
{
    int returnResult;
    if(laserCount==1)
    {
        switch (solverIn) {

        case E_MAP_SOLVER_GMAPPING:
            map_selector=0;
            returnResult=1;
            break;
        case E_MAP_SOLVER_CARTOGRAPHER:
            map_selector=1;
            returnResult=1;
            break;
        case E_MAP_SOLVER_HECTOR:
            map_selector=2;
            returnResult=1;
            break;
        default:
            map_selector=0;
            returnResult=0;
            break;
        }
    }
    else if(laserCount==2)
    {
        switch (solverIn) {

        case E_MAP_SOLVER_GMAPPING:
            map_selector=3;
            returnResult=1;
            break;
        case E_MAP_SOLVER_CARTOGRAPHER:
            map_selector=4;
            returnResult=1;
            break;
        case E_MAP_SOLVER_HECTOR:
            map_selector=5;
            returnResult=1;
            break;
        default:
            map_selector=3;
            returnResult=0;
            break;
        }
    }
    return returnResult;
}

//选择odom
int RosGuiApi::setOdomSolver(struct UserInfo userInfo,enum E_ODOM_SOLVER solverIn,int &returnFlag)
{
    int returnResult;
    std::string odom_algorithm;
    switch (solverIn) {
    case E_ODOM_SOLVER_ENCODER:
        //base_link_to_front_laser is a required node
        system("rosnode kill /base_link_to_front_laser &");
        odom_algorithm="roslaunch tcr100 odom_from_encoder.launch &";
        break;

    case E_ODOM_SOLVER_LASER:
        system("rosnode kill /base_link_to_front_laser &");
        odom_algorithm="roslaunch tcr100 odom_from_laser.launch &";
        break;
    case E_ODOM_SOLVER_ENCODER_IMU:
        system("rosnode kill /base_link_to_front_laser &");
        odom_algorithm="roslaunch tcr100 odom_from_encoder_and_imu.launch &";
        break;
    case E_ODOM_SOLVER_LASER_ENCODER_IMU:
        system("rosnode kill /base_link_to_front_laser &");
        odom_algorithm="roslaunch tcr100 odom_from_encoder_laser_and_imu.launch &";
        break;
    default:
        //system("rosnode kill /base_link_to_front_laser &");
        odom_algorithm="roslaunch tcr100 odom_from_encoder.launch &";
        returnResult=0;
        break;
    }
    const char* odom_algorithm_sh=odom_algorithm.c_str();
    int odom_status=system(odom_algorithm_sh);
    if(odom_status==-1)
    {
        printf("odom luanch system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(odom_status))
        {
            if(0==WEXITSTATUS(odom_status))
            {
                printf("odom luanch run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("odom luanch run failed %d \n",WEXITSTATUS(odom_status));
                returnResult=0;
            }
        }
        else
        {
            printf("odom luanch exit code %d \n",WEXITSTATUS(odom_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//开始建图
int RosGuiApi::createMapStart(struct UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string mapping_algorithm;
    if(map_selector==0)
    {
        mapping_algorithm="roslaunch tcr100 gmapping_with_one_lidar.launch &";
    }
    else if(map_selector==1)
    {
        mapping_algorithm="roslaunch tcr100 cartographer_with_one_lidar.launch &";
    }
    else if(map_selector==2)
    {
        mapping_algorithm="roslaunch tcr100 hector_with_one_lidar.launch &";
    }
    else if(map_selector==3)
    {
        mapping_algorithm="roslaunch tcr100 gmapping_with_two_lidars.launch &";
    }
    else if(map_selector==4)
    {
        mapping_algorithm="roslaunch tcr100 cartographer_with_two_lidars.launch &";
    }
    else if(map_selector==5)
    {
        mapping_algorithm="roslaunch tcr100 hector_with_two_lidars.launch &";
    }
    else
    {
        returnResult=0;
    }
    const char* mapping_algorithm_sh=mapping_algorithm.c_str();
    int mapping_algorithm_status=system(mapping_algorithm_sh);
    if(mapping_algorithm_status==-1)
    {
        printf("mapping_algorithm system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(mapping_algorithm_status))
        {
            if(0==WEXITSTATUS(mapping_algorithm_status))
            {
                printf("mapping_algorithm run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("mapping_algorithm run failed %d \n",WEXITSTATUS(mapping_algorithm_status));
                returnResult=0;
            }
        }
        else
        {
            printf("mapping_algorithm exit code %d \n",WEXITSTATUS(mapping_algorithm_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//结束建图
int RosGuiApi::createMapEnd(struct UserInfo userInfo,std::string mapName,int &returnFlag)
{
    int returnResult;
    std::string mapFile_path=FILE_PATH;
    std::string map_path;
    if(map_selector==0)
    {
        map_path="map_saver_gmapping_one.sh ";
    }
    else if(map_selector==1)
    {
        map_path="map_saver_cartographer_one.sh ";
    }
    else if(map_selector==2)
    {
        map_path="map_saver_hector_one.sh ";
    }
    else if(map_selector==3)
    {
        map_path="map_saver_gmapping_two.sh ";
    }
    else if(map_selector==4)
    {
        map_path="map_saver_cartographer_two.sh ";
    }
    else if(map_selector==5)
    {
        map_path="map_saver_hector_two.sh ";
    }
    else
    {
        returnResult=0;
    }
    mapFile_path+=map_path;
    mapFile_path+=mapName;
    mapFile_path+="&";

    const char* map_saver_name=mapFile_path.c_str();
    printf("map_saver cmd: %s \n",map_saver_name);
    int map_saver_status=system(map_saver_name);
    if(map_saver_status==-1)
    {
        printf("map_saver system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(map_saver_status))
        {
            if(0==WEXITSTATUS(map_saver_status))
            {
                printf("map_saver run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("map_saver run failed %d \n",WEXITSTATUS(map_saver_status));
                returnResult=0;
            }
        }
        else
        {
            printf("map_saver exit code %d \n",WEXITSTATUS(map_saver_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//加载建图
int RosGuiApi::loadMap(struct UserInfo userInfo,std::string mapName,int &returnFlag)
{
    int returnResult;
    std::string systemCommand="roslaunch tcr100 map_server.launch map:=";
    systemCommand+=mapName;
    systemCommand+=" &";
    map_to_load=systemCommand.c_str();
    int map_loader_status=system(map_to_load);
    if(map_loader_status==-1)
    {
        printf("map_loader system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(map_loader_status))
        {
            if(0==WEXITSTATUS(map_loader_status))
            {
                printf("map_loader run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("map_loader run failed %d \n",WEXITSTATUS(map_loader_status));
                returnResult=0;
            }
        }
        else
        {
            printf("map_loader exit code %d \n",WEXITSTATUS(map_loader_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//选择定位算法
int RosGuiApi::setLocationSolver(struct UserInfo userInfo,enum E_LOCATION_SOLVER solverIn,int &returnFlag)
{
    int returnResult;

    switch (solverIn) {
    case E_LOCATION_SOLVER_AMCL:
        localization_algorithm=0;
        returnResult=1;
        break;
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER:
        localization_algorithm=1;
        returnResult=1;
        break;
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER_IMU:
        localization_algorithm=2;
        returnResult=1;
        break;
    default:
        localization_algorithm=0;
        returnResult=0;
        break;
    }

    return returnResult;
}


//选择全局规划器
int RosGuiApi::setTrajectoryGlobalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_GLOBAL_SOLVER solverIn,int &returnFlag)
{
    int returnResult;
    switch (solverIn) {
    case E_TRAJECTORY_GLOBAL_SOLVER_ASTAR:
        global_trajectory_selector=0;
        returnResult=1;
        break;
    case E_TRAJECTORY_GLOBAL_SOLVER_DIJKSTRAS:
        global_trajectory_selector=1;
        returnResult=1;
        break;
    default:
        global_trajectory_selector=0;
        returnResult=0;
        break;
    }
    return returnResult;
}

//选择局部规划器
int RosGuiApi::setTrajectoryLocalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_LOCAL_SOLVER solverIn,int &returnFlag)
{
    int returnResult;
    switch (solverIn) {
    case E_TRAJECTORY_LOCAL_SOLVER_DWA:
        local_trajectory_selector=0;
        returnResult=1;
        break;
    case E_TRAJECTORY_LOCAL_SOLVER_BASELOCAL:
        local_trajectory_selector=1;
        returnResult=1;
        break;
    case E_TRAJECTORY_LOCAL_SOLVER_TEB:
        local_trajectory_selector=2;
        returnResult=1;
        break;
    default:
        local_trajectory_selector=0;
        returnResult=0;
        break;
    }
    return returnResult;
}

//start 导航
int RosGuiApi::startNavigation(struct UserInfo userInfo,int &returnFlag)//start navigation
{
    int returnResult;
    //start the localization algorithm
    std::string navigation_algorithm;
    if(localization_algorithm==0)
    {
        returnResult=1;
        if(global_trajectory_selector==0 && local_trajectory_selector==0)
        {
            ROS_INFO("astar and dwa is choosed \n");
            navigation_algorithm="roslaunch tcr100 move_base_astar_daw.launch &";
        }
    }
    const char* navigation_algorithm_sh=navigation_algorithm.c_str();
    int start_navigation_status=system(navigation_algorithm_sh);
    if(start_navigation_status==-1)
    {
        printf("start_navigation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(start_navigation_status))
        {
            if(0==WEXITSTATUS(start_navigation_status))
            {
                printf("start_navigation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("start_navigation run failed %d \n",WEXITSTATUS(start_navigation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("start_navigation exit code %d \n",WEXITSTATUS(start_navigation_status));
            returnResult=0;
        }
    }

    return returnResult;
}
//stop 导航
int RosGuiApi::stopNavigation(struct UserInfo userInfo,int &returnFlag)//start navigation
{
    int returnResult;
    std::string stop_navigation=FILE_PATH;
    std::string file_path;
    file_path="stop_navigation.sh &";
    stop_navigation+=file_path;
    const char* stop_navigation_sh=stop_navigation.c_str();
    int stop_navigation_status=system(stop_navigation_sh);
    if(stop_navigation_status==-1)
    {
        printf("stop_navigation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(stop_navigation_status))
        {
            if(0==WEXITSTATUS(stop_navigation_status))
            {
                printf("stop_navigation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("stop_navigation run failed %d \n",WEXITSTATUS(stop_navigation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("stop_navigation exit code %d \n",WEXITSTATUS(stop_navigation_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//获取agv位置
int RosGuiApi::getVehiclePosition(struct UserInfo userInfo,struct VehiclePosition& positionReturn)
{
    int returnResult=1;

    positionReturn=rosVehiclePosition;
    return returnResult;
}

//设置agv初始位置
int RosGuiApi::setVehiclePosition(struct UserInfo userInfo,struct VehiclePosition positionIn,int &returnFlag)
{
    int returnResult;
    geometry_msgs::PoseWithCovarianceStamped init_pose;
    int count=0;
    while(ros::ok()&&count<3)
    {
        rate_.sleep();
        ros::spinOnce();
        init_pose.header.frame_id = "map";
        init_pose.header.stamp = ros::Time::now();
        init_pose.pose.pose.position.x =positionIn.x ;
        init_pose.pose.pose.position.y =positionIn.y ;
        geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(positionIn.rotate);
        init_pose.pose.pose.orientation =pose_quat ;
        ROS_INFO("initial position: %.2f, %.2f, %.2f",
                 init_pose.pose.pose.position.x, init_pose.pose.pose.position.y,
                 tf::getYaw(init_pose.pose.pose.orientation));
        initial_position_pub_.publish(init_pose);
        returnResult=1;
        count++;
    }
    return returnResult;
}

//设置失败重试次数
int RosGuiApi::setReplanCoutLimitWhenFailure(struct UserInfo userInfo,int countLimit,int &returnFlag)
{
    int returnResult;
    std::string faile_retry_str;
    stringstream faile_retry_ss;
    faile_retry_ss<<countLimit;
    faile_retry_ss>>faile_retry_str;
    std::string faile_retry_file="rosrun tcr100 nav_state.py _repeat_num:=";
    faile_retry_file+=faile_retry_str;
    faile_retry_file+=" &";
    const char* faile_retry_sh=faile_retry_file.c_str();
    int faile_retry_status=system(faile_retry_sh);
    if(faile_retry_status==-1)
    {
        printf("faile_retry system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(faile_retry_status))
        {
            if(0==WEXITSTATUS(faile_retry_status))
            {
                printf("faile_retry run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("faile_retry run failed %d \n",WEXITSTATUS(faile_retry_status));
                returnResult=0;
            }
        }
        else
        {
            printf("faile_retry exit code %d \n",WEXITSTATUS(faile_retry_status));
            returnResult=0;
        }
    }

    return returnResult;
}

////导航到指定点
//int RosGuiApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
//{
//    int returnResult;
//    bool goalReached=false;
//    //    while ((move_base_client_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
//    //    {
//    //      ROS_WARN_THROTTLE(1, "Waiting for move_base action server to come up...");
//    //    }
//    setVelocity(velocity);
//    if(goalReached=moveToGoal(positionIn))
//    {
//        returnResult=1;
//        ros::spinOnce();
//    }
//    else
//    {
//        returnResult=0;
//    }
//    return returnResult;
//}


////导航到指定点
//int RosGuiApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
//{
//    int returnResult;
//    setVelocity(velocity);

//    std::string x_str;
//    stringstream x_ss;
//    x_ss<<positionIn.x;
//    x_ss>>x_str;
//    std::string y_str;
//    stringstream y_ss;
//    y_ss<<positionIn.y;
//    y_ss>>y_str;
//    std::string r_str;
//    stringstream r_ss;
//    r_ss<<positionIn.rotate;
//    r_ss>>r_str;

//    std::string goal_file="/home/tcr/tcr100_ws/src/tcr100/scripts/goal_sending.py ";
//    goal_file+=x_str;
//    goal_file+=" ";
//    goal_file+=y_str;
//    goal_file+=" ";
//    goal_file+=r_str;
//    goal_file+=" &";
//    const char* goal_sh=goal_file.c_str();
//    int set_goal_status=system(goal_sh);
//    printf("goal sending command: %s \n",goal_sh);
//    if(set_goal_status==-1)
//    {
//        printf("set_velocity system error... \n");
//        returnResult=0;
//    }
//    else
//    {
//        if(WIFEXITED(set_goal_status))
//        {
//            if(0==WEXITSTATUS(set_goal_status))
//            {
//                printf("set_goal run successfully... \n");
//                returnResult=1;
//            }
//            else
//            {
//                printf("set_goal run failed %d \n",WEXITSTATUS(set_goal_status));
//                returnResult=0;
//            }
//        }
//        else
//        {
//            printf("set_velocity exit code %d \n",WEXITSTATUS(set_goal_status));
//            returnResult=0;
//        }
//    }
//    return returnResult;

//}


//导航到指定点
int RosGuiApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
{
    tcr100::VehicleGoal msg;
    setVelocity(velocity);
    msg.position_x=positionIn.x;
    msg.position_y=positionIn.y;
    msg.position_rotate=positionIn.rotate;
    goal_pub_.publish(msg);

    return 0;
}

//多点巡航
int RosGuiApi::moveFollowPath(struct UserInfo userInfo,std::string filePath,std::string trajectoryName,double nearbyDistance,double velocity,int &returnFlag)
{
    int returnResult;
    setVelocity(velocity);
    setNearbyDistance(nearbyDistance);


    return returnResult;
}

//设置速度
int RosGuiApi::changeVelocity(struct UserInfo userInfo,double velocity,int &returnFlag)
{
    int returnResult;
    if(setVelocity(velocity)==1)
    {
        returnResult=1;
    }
    else
    {
        returnResult=0;
    }
    return returnResult;
}



//获取导航状态
int RosGuiApi::getMovingStatus(struct UserInfo userInfo,enum E_GOAL_STATUS& status)
{
    switch(nav_status)
    {
    case 0:
        status=E_GOAL_STATUS_PENDING;
        break;
    case 1:
        status=E_GOAL_STATUS_ACTIVE;
        break;
    case 2:
        status=E_GOAL_STATUS_PREEMPTED;
        break;
    case 3:
        status=E_GOAL_STATUS_SUCCEEDED;
        break;
    case 4:
        status=E_GOAL_STATUS_ABORTED;
        break;
    case 5:
        status=E_GOAL_STATUS_REJECTED;
        break;
    case 6:
        status=E_GOAL_STATUS_PREEMPTING;
        break;
    case 7:
        status=E_GOAL_STATUS_RECALLING;
        break;
    case 8:
        status=E_GOAL_STATUS_RECALLED;
        break;
    case 9:
        status=E_GOAL_STATUS_LOST;
        break;
    default:
        status=E_GOAL_STATUS_PENDING;
        break;
    }
    return 0;
}

//获取剩余时间
int RosGuiApi::getRemainningTime(struct UserInfo userInfo,double &timeOut)
{
    int returnResult;
    returnResult=1;
    timeOut=path_lenght/rosVehicleCmdVel.x_move;
    return returnResult;
}

//获取剩余距离
int RosGuiApi::getRemainningDistance(struct UserInfo userInfo,double &distanceOut)
{
    int returnResult;
    returnResult=1;
    distanceOut=path_lenght;
    return returnResult;
}

//取消当前导航目标
int RosGuiApi::cancelMotion(struct UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_goal;
    cancel_goal="rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID {} ";
    const char* cancel_goal_sh=cancel_goal.c_str();
    int status=system(cancel_goal_sh);
    if(status==-1)
    {
        printf("cancel goal system error... \n");
    }
    else
    {
        if(WIFEXITED(status))
        {
            if(0==WEXITSTATUS(status))
            {
                printf("cancel goal run successfully... \n");
                nav_status=0;
                returnResult=1;
            }
            else
            {
                printf("cancel goal run failed %d \n",WEXITSTATUS(status));
            }
        }
        else
        {
            printf("cancel goal exit code %d \n",WEXITSTATUS(status));
        }
    }
    return returnResult;
}
//设置导航允许误差
int RosGuiApi::setPositionArrivedAccuracy(struct UserInfo userInfo,double distanceErrorIn,double angularErrorIn,int &returnFlag)
{
    int returnResult;
    std::string distance_str,yaw_str;
    stringstream dis_ss,yaw_ss;
    dis_ss<<distanceErrorIn;
    dis_ss>>distance_str;

    yaw_ss<<angularErrorIn;
    yaw_ss>>yaw_str;
    std::string distance_file=FILE_PATH;
    std::string file_path="change_distance_accuracy.sh ";
    distance_file+=file_path;
    distance_file+=distance_str;
    distance_file+=" ";
    distance_file+=yaw_str;
    distance_file+=" &";
    const char* distance_sh=distance_file.c_str();
    int distance_status=system(distance_sh);
    if(distance_status==-1)
    {
        printf("change location accuracy system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(distance_status))
        {
            if(0==WEXITSTATUS(distance_status))
            {
                printf("change location accuracy run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("change location accuracy run failed %d \n",WEXITSTATUS(distance_status));
                returnResult=0;
            }
        }
        else
        {
            printf("change location accuracy exit code %d \n",WEXITSTATUS(distance_status));
            returnResult=0;
        }
    }
    return returnResult;
}


//设置局部地图大小
int RosGuiApi::setLocalPlanRange(struct UserInfo userInfo,double x_rangeIn,double y_rangeIn,int &returnFlag)
{
    int returnResult;
    std::string param_str[4];
    stringstream param_ss[3];
    param_ss[0]<<x_rangeIn;
    param_ss[1]<<y_rangeIn;
    for(int j=0;j<2;j++)
    {
        param_ss[j]>>param_str[j];
    }
    std::string local_range_file=FILE_PATH;
    std::string file_path="local_range.sh ";
    local_range_file+=file_path;
    for(int i=0;i<2;i++)
    {
        if(i!=0)
        {
            local_range_file+=" ";
        }
        local_range_file+=param_str[i];
    }
    local_range_file+=" &";
    const char* local_range_sh=local_range_file.c_str();
    int local_range_status=system(local_range_sh);
    if(local_range_status==-1)
    {
        printf("set local_range system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(local_range_status))
        {
            if(0==WEXITSTATUS(local_range_status))
            {
                printf("set local_range run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set local_range run failed %d \n",WEXITSTATUS(local_range_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set local_range exit code %d \n",WEXITSTATUS(local_range_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//设置车体的footprint
int RosGuiApi::setVehicleBorder(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag)
{
    int returnResult;
    int size=point_x.size();
    int length=6;
    std::string param_str[100];
    stringstream param_ss[100];
    for(int j=0;j<size;j++)
    {
        if(j!=0)
        {
            param_ss[j*length-1]<<",";
        }
        for(int i=0;i<length-1;i++)
        {
            if(i==0)
            {
                param_ss[i+j*length]<<"[";
            }
            if(i==1)
            {
                param_ss[i+j*length]<<point_x[j];
            }
            if(i==2)
            {
                param_ss[i+j*length]<<",";
            }
            if(i==3)
            {
                param_ss[i+j*length]<<point_y[j];
            }  if(i==4)
            {
                param_ss[i+j*length]<<"]";
            }
        }
    }
    param_ss[size*length-1]<<"]";
    for(int k=0;k<=size*length-1;k++)
    {
        param_ss[k]>>param_str[k];
    }
    std::string footprint_file="rosrun dynamic_reconfigure dynparam set /move_base/local_costmap footprint '[";
    for(int m=0;m<=size*length;m++)
    {
        footprint_file+=param_str[m];
    }
    footprint_file+="'";
    footprint_file+=" &";
    const char* footprint_sh=footprint_file.c_str();
    printf("command: %s \n",footprint_sh);
    int footprint_status=system(footprint_sh);
    if(footprint_status==-1)
    {
        printf("set footprint system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(footprint_status))
        {
            if(0==WEXITSTATUS(footprint_status))
            {
                printf("set footprint run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set footprint run failed %d \n",WEXITSTATUS(footprint_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set footprint exit code %d \n",WEXITSTATUS(footprint_status));
            returnResult=0;
        }
    }
    return returnResult;
}


//设置local膨胀区大小
int RosGuiApi::setLocalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag)
{
    int returnResult;
    std::string local_inflation_str;
    stringstream local_inflation_ss;
    local_inflation_ss<<offsetIn;
    local_inflation_ss>>local_inflation_str;
    std::string local_inflation_file="rosrun dynamic_reconfigure dynparam set /move_base/local_costmap/inflation_layer inflation_radius ";
    local_inflation_file+=local_inflation_str;
    local_inflation_file+=" &";
    const char* local_inflation_sh=local_inflation_file.c_str();
    int local_inflation_status=system(local_inflation_sh);
    if(local_inflation_status==-1)
    {
        printf("set local_inflation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(local_inflation_status))
        {
            if(0==WEXITSTATUS(local_inflation_status))
            {
                printf("set local_inflation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set local_inflation run failed %d \n",WEXITSTATUS(local_inflation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set local_inflation exit code %d \n",WEXITSTATUS(local_inflation_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//设置global膨胀区大小
int RosGuiApi::setGlobalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag)
{
    int returnResult;
    std::string global_inflation_str;
    stringstream global_inflation_ss;
    global_inflation_ss<<offsetIn;
    global_inflation_ss>>global_inflation_str;
    std::string global_inflation_file="rosrun dynamic_reconfigure dynparam set /move_base/global_costmap/inflation_layer inflation_radius ";
    global_inflation_file+=global_inflation_str;
    global_inflation_file+=" &";
    const char* global_inflation_sh=global_inflation_file.c_str();
    int global_inflation_status=system(global_inflation_sh);
    if(global_inflation_status==-1)
    {
        printf("set global_inflation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(global_inflation_status))
        {
            if(0==WEXITSTATUS(global_inflation_status))
            {
                printf("set global_inflation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set global_inflation run failed %d \n",WEXITSTATUS(global_inflation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set global_inflation exit code %d \n",WEXITSTATUS(global_inflation_status));
            returnResult=0;
        }
    }
    return returnResult;
}
////重置
//int RosGuiApi::resetAll(struct UserInfo userInfo,int &returnFlag)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}
////获取ROS包的状态
//int RosGuiApi::getRosPackageStatus(enum E_ROS_PACKAGE package,enum E_ROS_PACKAGE_STATUS &status)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}

int RosGuiApi::addVitrualBlock(struct UserInfo userInfo,std::string configFilePath,int &returnFlag)
{
    int returnResult;
    std::string virtual_wall_str;
    stringstream virtual_wall_ss;
    virtual_wall_ss<<configFilePath;
    virtual_wall_ss>>virtual_wall_str;
    std::string virtual_wall_file="roslaunch yocs_virtual_sensor standalone.launch virtual_wall:=";
    virtual_wall_file+=virtual_wall_str;
    virtual_wall_file+=" &";
    const char* virtual_wall_sh=virtual_wall_file.c_str();
    int virtual_wall_status=system(virtual_wall_sh);
    if(virtual_wall_status==-1)
    {
        printf("set virtual_wall system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(virtual_wall_status))
        {
            if(0==WEXITSTATUS(virtual_wall_status))
            {
                printf("set virtual_wall run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set virtual_wall run failed %d \n",WEXITSTATUS(virtual_wall_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set virtual_wall exit code %d \n",WEXITSTATUS(virtual_wall_status));
            returnResult=0;
        }
    }
    return returnResult;
}

int RosGuiApi::deleteVitrualBlock(struct UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_virtual_wall=FILE_PATH;
    std::string file_path;
    file_path="delete_virtual_wall.sh &";
    cancel_virtual_wall+=file_path;
    const char* cancel_virtual_wall_sh=cancel_virtual_wall.c_str();
    int cancel_virtual_wall_status=system(cancel_virtual_wall_sh);
    if(cancel_virtual_wall_status==-1)
    {
        printf("cancel_virtual_wall system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(cancel_virtual_wall_status))
        {
            if(0==WEXITSTATUS(cancel_virtual_wall_status))
            {
                printf("cancel_virtual_wall run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("cancel_virtual_wall run failed %d \n",WEXITSTATUS(cancel_virtual_wall_status));
                returnResult=0;
            }
        }
        else
        {
            printf("cancel_virtual_wall exit code %d \n",WEXITSTATUS(cancel_virtual_wall_status));
            returnResult=0;
        }
    }

    return returnResult;
}

//start waypoints follow
int RosGuiApi::startPathFollowMotion(UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    int waypoints_follow_status=system("rosrun yocs_waypoints nav_ctrl_pub.py &");
    if(waypoints_follow_status==-1)
    {
        printf("waypoints_follow start system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(waypoints_follow_status))
        {
            if(0==WEXITSTATUS(waypoints_follow_status))
            {
                printf("waypoints_follow start run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("waypoints_follow start run failed %d \n",WEXITSTATUS(waypoints_follow_status));
                returnResult=0;
            }
        }
        else
        {
            printf("waypoints_follow start exit code %d \n",WEXITSTATUS(waypoints_follow_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//pause motion
int  RosGuiApi::ceaseMotion(UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_goal;
    cancel_goal="rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID {} ";
    const char* cancel_goal_sh=cancel_goal.c_str();
    int status=system(cancel_goal_sh);
    if(status==-1)
    {
        printf("cancel goal system error... \n");
    }
    else
    {
        if(WIFEXITED(status))
        {
            if(0==WEXITSTATUS(status))
            {
                printf("cancel goal run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("cancel goal run failed %d \n",WEXITSTATUS(status));
            }
        }
        else
        {
            printf("cancel goal exit code %d \n",WEXITSTATUS(status));
        }
    }
    return returnResult;

}


int RosGuiApi::startMotion(UserInfo userInfo,int &returnFlag)
{
    current_goal.target_pose.header.frame_id = "map";
    current_goal.target_pose.header.stamp = ros::Time::now();
    ROS_INFO("Continue the pevious goal location ...");
    move_base_client_.sendGoal(current_goal);
    move_base_client_.waitForResult();
    if(move_base_client_.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Goal have finally been reached!");
        return true;
    }
    else
    {
        ROS_INFO("The robot failed to reach complete the cotinune navigation");
        return false;
    }

}
//int RosGuiApi::addForbidenZone(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}



//**********functions defined by myself*************//
void RosGuiApi::moveBaseResultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr& msg)
{
    if (!msg->status_list.empty())
    {
        actionlib_msgs::GoalStatus goalStatus=msg->status_list[0];
        ROS_INFO("got goal state!");
        if(goalStatus.status==1)
        {
            ROS_INFO("goal started!");
            nav_status=1;
        }
        else if(goalStatus.status==2)
        {
            ROS_INFO("goal preemmpted!");
            nav_status=2;
        }
        else if(goalStatus.status==3)
        {
            ROS_INFO("goal succeeded!");
            nav_status=3;
        }
        else if(goalStatus.status==4)
        {
            ROS_INFO("goal aborted!");
            nav_status=4;
        }
        else if(goalStatus.status==5)
        {
            ROS_INFO("goal rejected!");
            nav_status=5;
        }
        else if(goalStatus.status==6)
        {
            ROS_INFO("goal preempting!");
            nav_status=6;
        }
        else if(goalStatus.status==7)
        {
            ROS_INFO("goal recalling!");
            nav_status=7;
        }
        else if(goalStatus.status==8)
        {
            ROS_INFO("goal recalled!");
            nav_status=8;
        }
        else if(goalStatus.status==9)
        {
            ROS_INFO("goal lost!");
            nav_status=9;
        }
    }
    else
    {
        ROS_INFO("goal pending!");
        nav_status=0;
    }
}


//set velocity
int RosGuiApi::setVelocity(double velocity)
{
    int returnResult;
    std::string str;
    stringstream ss;
    ss<<velocity;
    ss>>str;
    //std::string velocity_file=FILE_PATH;
    std::string velocity_file="rosrun dynamic_reconfigure dynparam set /move_base/DWAPlannerROS min_vel_x ";
    //velocity_file+=file;
    velocity_file+=str;
    velocity_file+=" &";
    const char* velocity_sh=velocity_file.c_str();
    int set_velocity_status=system(velocity_sh);
    printf("vel change command: %s \n",velocity_sh);
    if(set_velocity_status==-1)
    {
        printf("set_velocity system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(set_velocity_status))
        {
            if(0==WEXITSTATUS(set_velocity_status))
            {
                printf("set_velocity run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set_velocity run failed %d \n",WEXITSTATUS(set_velocity_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set_velocity exit code %d \n",WEXITSTATUS(set_velocity_status));
            returnResult=0;
        }
    }
    return returnResult;
}


int RosGuiApi::setNearbyDistance(double distance)
{
    int returnResult;
    std::string nearby_distance_str;
    stringstream nearby_distance_ss;
    nearby_distance_ss<<distance;
    nearby_distance_ss>>nearby_distance_str;
    //std::string velocity_file=FILE_PATH;
    std::string nearby_distance_file="rosrun dynamic_reconfigure dynparam set /waypoints_navi close_enough ";
    //velocity_file+=file;
    nearby_distance_file+=nearby_distance_str;
    nearby_distance_file+=" &";
    const char* nearby_distance_sh=nearby_distance_file.c_str();
    int nearby_distance_status=system(nearby_distance_sh);
    if(nearby_distance_status==-1)
    {
        printf("nearby_distance system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(nearby_distance_status))
        {
            if(0==WEXITSTATUS(nearby_distance_status))
            {
                printf("nearby_distance run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("nearby_distance run failed %d \n",WEXITSTATUS(nearby_distance_status));
                returnResult=0;
            }
        }
        else
        {
            printf("nearby_distance exit code %d \n",WEXITSTATUS(nearby_distance_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//Move to a signed position
bool RosGuiApi::moveToGoal(struct VehiclePosition positionIn)
{
    move_base_msgs::MoveBaseGoal goal;

    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x =positionIn.x ;
    goal.target_pose.pose.position.y =positionIn.y ;
    geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(positionIn.rotate);
    goal.target_pose.pose.orientation =pose_quat ;
    ROS_INFO("New goal: %.2f, %.2f, %.2f",
             goal.target_pose.pose.position.x, goal.target_pose.pose.position.y,
             tf::getYaw(goal.target_pose.pose.orientation));
    ROS_INFO("sending goal location ...");
    move_base_client_.sendGoal(goal);
    move_base_client_.waitForResult();
    if(move_base_client_.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Goal have been reached!");
        return true;
    }
    else
    {
        ROS_INFO("The robot failed to reach the destination");
        return false;
    }

}

//Get odometry from encoder
int RosGuiApi::setWheelOdometerData(UserInfo userInfo, int robotIdIn, OdemeterData odemeterDataReturn, int &returnFlag)
{
    tcr100::VehicleEncoder temp_msg;
    temp_msg.pos_x=odemeterDataReturn.pos.x;
    temp_msg.pos_y=odemeterDataReturn.pos.y;
    temp_msg.pos_rotate=odemeterDataReturn.pos.rotate;

    temp_msg.vel_x=odemeterDataReturn.vel.x_move;
    temp_msg.vel_y=odemeterDataReturn.vel.y_move;
    temp_msg.vel_rotate=odemeterDataReturn.vel.z_rotate;

    if(ros::ok())
    {
        encoder_pub_.publish(temp_msg);
    }
}

//get velocity from ROS
void RosGuiApi::twistCmdVelCallback(const geometry_msgs::Twist &msg)
{
    rosVehicleCmdVel.x_move=msg.linear.x;
    rosVehicleCmdVel.y_move=msg.linear.y;
    rosVehicleCmdVel.z_rotate=msg.angular.z;
    ROS_INFO("**********vehicle velocity cmd: %.2f, %.2f, %.2f",
             rosVehicleCmdVel.x_move, rosVehicleCmdVel.y_move,
             rosVehicleCmdVel.z_rotate);

}

//send velocity command to base control
int RosGuiApi::getVehicleVelocityComand(UserInfo userInfo, int robotIdIn, VehicleVelocity &velocityComamnd)
{
    //  velocityComamnd=rosVehicleCmdVel;
    velocityComamnd.x_move=rosVehicleCmdVel.x_move;
    velocityComamnd.y_move=rosVehicleCmdVel.y_move;
    velocityComamnd.z_rotate=rosVehicleCmdVel.z_rotate;
}

//calculate the lenght of global path
void RosGuiApi::gloablPathLenghtCallback(const nav_msgs::Path& path)
{

    int len=sizeof(path.poses)/sizeof(path.poses[0]);
    path_lenght=0;
    for(int i=0;i<len;i++)
    {
        path_lenght+=sqrt(pow((path.poses[i+1].pose.position.x - path.poses[i].pose.position.x),2) +
                pow((path.poses[i+1].pose.position.y - path.poses[i].pose.position.y), 2));
    }
}

//ROS spin
void RosGuiApi::freeSleep()
{
    while(ros::ok())
    {
        ros::spinOnce();
        getPositionFromROS();
        rate_.sleep();
    }
}

//subscribe to goal topic
void RosGuiApi::movebaseGoalCallback(const move_base_msgs::MoveBaseActionGoal& goal)
{
    current_goal=goal.goal;
}

void RosGuiApi::robotPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& pose)
{
    rosVehiclePosition.x =pose.pose.pose.position.x;
    rosVehiclePosition.y =pose.pose.pose.position.y;
    tf::Quaternion q(pose.pose.pose.orientation.x,pose.pose.pose.orientation.y,
                     pose.pose.pose.orientation.z,pose.pose.pose.orientation.w);
    double roll,pitch,yaw;
    tf::Matrix3x3 m(q);
    m.getRPY(roll,pitch,yaw);
    rosVehiclePosition.rotate = yaw;
}
int RosGuiApi::getPositionFromROS()
{
    int returnResult;
    vector<string> Vstring;
    ros::master::getNodes(Vstring);
    tf::TransformListener listener;

    tf::StampedTransform transform;
    bool tf_ok=true;

    if(((find(Vstring.begin(),Vstring.end(),"/move_base"))!=Vstring.end())&&
            ((find(Vstring.begin(),Vstring.end(),"/amcl"))!=Vstring.end()))
    {
            try
            {
                listener.waitForTransform("/map", "/base_link", ros::Time::now(), ros::Duration(2));
                listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
            }
            catch (tf::TransformException ex)
            {
                ROS_ERROR("%s", ex.what());
                ros::Duration(1.0).sleep();
                tf_ok=false;
            }
        if(tf_ok)
        {
            rosVehiclePosition.x =transform.getOrigin().getX();
            rosVehiclePosition.y =transform.getOrigin().getY();
            double yaw;
            tf::Quaternion q=transform.getRotation();
            yaw=tf::getYaw(q);
            rosVehiclePosition.rotate = yaw;
            printf("positionReturn: %f %f %f \n",rosVehiclePosition.x,rosVehiclePosition.y,rosVehiclePosition.rotate);
            returnResult=1;
        }
        else
        {
            returnResult=0;
        }
    }
    else
    {
        printf("/move_base has not start yet \n");
        return 0;
    }
}
